#include "Arduino.h"
#include "MiniBot.h"

//Declare helper functions
byte readByte(byte address);
void writeByte(byte address, byte data);
void InitializeSensors();
bool TestSensor();
void WriteParameters();
unsigned int ReadIRSensor();
unsigned int ReadLightSensor();
void SwitchToFrontSensor();
void SwitchToLeftSensor();
void SwitchToRightSensor();


//Set mux input pins
int s0 = 2;
int s1 = 3;
int s2 = 4;
int s3 = 5;

/*****************************************************************
*
* MiniBot Functions
*
*****************************************************************/

/////////////////////////////////////////////////
//Setup function for sensors (called in setup())
/////////////////////////////////////////////////
void MiniBot::Setup()
{
  //Set mux input pins for digital output
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT);  

  //Start serial monitor 
  Serial.begin(9600); 
  
  //Start i2c communication
  Wire.begin(); 
  
  //Test sensor 1
  InitializeSensors();                              
}

/////////////////////////////////////////////////
//Read Left Light Sensor
/////////////////////////////////////////////////
unsigned int MiniBot::ReadLeftLightSensor()
{
     //Switch to left sensor
     SwitchToLeftSensor();
     
     //Read light sensor
     return ReadLightSensor();
         
}

/////////////////////////////////////////////////
//Read Right Light Sensor
/////////////////////////////////////////////////
unsigned int MiniBot::ReadRightLightSensor()
{
     //Switch to right sensor
     SwitchToRightSensor();
     
     //Read light sensor
     return ReadLightSensor();
}

/////////////////////////////////////////////////
//Read Front Light Sensor
/////////////////////////////////////////////////
unsigned int MiniBot::ReadFrontLightSensor()
{
     //Switch to front sensor
     SwitchToFrontSensor();
     
     //Read light sensor
     return ReadLightSensor();
}

/////////////////////////////////////////////////
//Read Left IR Sensor
/////////////////////////////////////////////////
unsigned int MiniBot::ReadLeftIRSensor()
{
     //Switch to left sensor
     SwitchToLeftSensor();
     
     //Read IR sensor
     return ReadIRSensor();
}

/////////////////////////////////////////////////
//Read Right IR Sensor
/////////////////////////////////////////////////
unsigned int MiniBot::ReadRightIRSensor()
{
     //Switch to right sensor
     SwitchToRightSensor(); 
     
     //Read IR sensor
     return ReadIRSensor();
}

/////////////////////////////////////////////////
//Read Front IR Sensor
/////////////////////////////////////////////////
unsigned int MiniBot::ReadFrontIRSensor()
{
     //Switch to front sensor
     SwitchToFrontSensor();  
     
     //Read IR sensor
     return ReadIRSensor();      
}


/*****************************************************************
*
* Helper Functions
*
*****************************************************************/

/////////////////////////////////////////////////
//Write byte
/////////////////////////////////////////////////
void writeByte(byte address, byte data)
{     
  Wire.beginTransmission(VCNL4000_ADDRESS);
  Wire.write(address);
  Wire.write(data);
  Wire.endTransmission();
}

/////////////////////////////////////////////////
//Read byte
/////////////////////////////////////////////////
byte readByte(byte address)
{
  //Declare data variable
  byte data;
  
  //Start data transmission
  Wire.beginTransmission(VCNL4000_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();
  Wire.requestFrom(VCNL4000_ADDRESS, 1);
    
  //Wait until bus is available     
  while(!Wire.available())
  {                                               
    //Do nothing      
  }

  //Read data
  data = Wire.read();

  //Return data
  return data;
}

/////////////////////////////////////////////////
//Read IR Sensor
/////////////////////////////////////////////////
unsigned int ReadIRSensor()
{
  //Declare variables
  unsigned int data;
  byte temp;
  
  //Read command register
  temp = readByte(COMMAND_0);

  //Command the sensor to perform a proximity measure
  writeByte(COMMAND_0, temp | 0x08);
  
  //Read command register and wait for data to be ready
  while(!(readByte(COMMAND_0)&0x20))
  {  
     //Do nothing
  }
  
  //Read first byte of IR data and shift left 8 bits
  data = readByte(PROXIMITY_RESULT_MSB) << 8;
  
  //Read second byte of IR data and OR with previous data
  data |= readByte(PROXIMITY_RESULT_LSB);
  
  //Read data
  return data;
}

/////////////////////////////////////////////////
//Read Light Sensor
/////////////////////////////////////////////////
unsigned int ReadLightSensor()
{
  //Declare variables
  unsigned int data;
  byte temp;
  
  //Read command register
  temp = readByte(COMMAND_0);
  
  //Command the sensor to perform ambient measure
  writeByte(COMMAND_0, temp | 0x10);  
  
  //Read command register and wait for data to be ready
  while(!(readByte(COMMAND_0)&0x40)) 
  {
      //Do nothing
  }
  
  //Read first byte of light data and shift left 8 bits
  data = readByte(AMBIENT_RESULT_MSB) << 8;
  
  //Read second byte of light data and OR with previous data
  data |= readByte(AMBIENT_RESULT_LSB);
  
  //Return data  
  return data;
}



/////////////////////////////////////////////////
//Initialize Sensors
/////////////////////////////////////////////////
void InitializeSensors()
{
     //Switch to left sensor
     SwitchToLeftSensor();
     
     //Test left sensor
     if (!TestSensor())
     {
        Serial.println("Error initializing left sensor");
     }
     else
     {
         Serial.println("Left sensor online...");         
     }
     
     //Write left sensor parameters
     WriteParameters();
     
     //Switch to right sensor
     SwitchToRightSensor();
     
     //Test right sensor
     if (!TestSensor())
     {
        Serial.println("Error initializing right sensor");
     }
     else
     {
         Serial.println("Right sensor online...");         
     }
     
     //Write right sensor parameters
     WriteParameters();
     
     ////Switch to front sensor
     SwitchToFrontSensor();
     
     //Test front sensor
     if (!TestSensor())
     {
         Serial.println("Error initializing front sensor");
     }
     else
     {
         Serial.println("Front sensor online...");         
     }
     
     //Write front sensor parameters
     WriteParameters();
     
         
}

/////////////////////////////////////////////////
//Intialize Sensor
/////////////////////////////////////////////////
bool TestSensor()
{
     //Read product ID
     byte temp = readByte(PRODUCT_ID);
     
     //Make sure product ID is correct (0x11)
     if (temp != 0x11)
     {
        return false;        
     }
     else
     {
         return true;          
     }        
}

/////////////////////////////////////////////////
//Write Parameters
/////////////////////////////////////////////////
void WriteParameters()
{
      //Set sensor parameters
      writeByte(AMBIENT_PARAMETER, 0x0F);  // Single conversion mode, 128 averages
      writeByte(IR_CURRENT, 20);  // Set IR current to 200mA
      writeByte(PROXIMITY_FREQ, 2);  // 781.25 kHz
      writeByte(PROXIMITY_MOD, 0x81);  // 129, recommended by Vishay 
}

/////////////////////////////////////////////////
//Switch to left sensor
/////////////////////////////////////////////////
void SwitchToLeftSensor()
{
     digitalWrite(s0, LOW);
     digitalWrite(s1, LOW);
     digitalWrite(s2, LOW);
     digitalWrite(s3, LOW);
}

/////////////////////////////////////////////////
//Switch to right sensor
/////////////////////////////////////////////////
void SwitchToRightSensor()
{
     digitalWrite(s0, HIGH);
     digitalWrite(s1, HIGH);
     digitalWrite(s2, HIGH);
     digitalWrite(s3, HIGH);
}

/////////////////////////////////////////////////
//Switch to front sensor
/////////////////////////////////////////////////
void SwitchToFrontSensor()
{
     digitalWrite(s0, LOW);
     digitalWrite(s1, HIGH);
     digitalWrite(s2, HIGH);
     digitalWrite(s3, HIGH);
}


